// Tree.cpp: implementation for the tree class.
// Copyright: (c) Project 5 Team (715), 2009
//////////////////////////////////////////////////////////////////////
#include "StdAfx.h"
#include "Tree.h"
#include <list>

namespace LifeSketchTree
{
bool compare_double (double first, double second)
{
	if(first <= second)
		return true;
	else 
		return false;
}

CTree::CTree()
{
	root2D = NULL;
	root3D = NULL;
	display_index      = 0;
	treeMidPoint       = 0;
	sketch_area_width  = 1;
	sketch_area_height = 1;
}

CTree::~CTree()
{
	if(root2D != NULL)
		delete root2D;
	glDeleteLists(display_index, 1);
}

void CTree::setTreeMidHeight(int midPoint)
{
	this->treeMidPoint = midPoint;
}

void CTree::setSketchArea(int sketch_area_width, int sketch_area_height)
{

	this->sketch_area_width  = sketch_area_width  ? sketch_area_width  : 1;
	this->sketch_area_height = sketch_area_height ? sketch_area_height : 1;
}

// dummy method which transparently uses a display list
void CTree::_display3D()
{
	if(display_index==0)
		display_index=glGenLists(1);

	glNewList(display_index, GL_COMPILE);
	display3D();
	glEndList();

	glCallList(display_index);
}

void CTree::display3D()
{
	if(root2D != NULL) {
		_drawTree3D(root3D);
	}
}

double CTree::_getBranchLength(CNode3D* node) {
	return _getBranchLength(node, node->getLine()->size()-1);
}

double CTree::_getBranchLength(CNode3D* node, int ptIndex) {
	double totlength = 0;
	for (int i = 0 ; i < ptIndex; i++) {
		totlength += _lineLength3D(node->getLine()->at(i), node->getLine()->at(i+1));
	}
	return totlength;
}

#define TRUNK_RADIUS 0.0275
#define TIP_RADIUS 0.0025
double CTree::_getBranchRadius(CNode3D* node, int ptIndex)
{
	double baseRadius = (node == root3D) ? TRUNK_RADIUS : 0.5*_getBranchRadius(node->getParent(), node->getParentIntersection()+1);
	double endRadius = TIP_RADIUS;
	double interpolation = 1-_getBranchLength(node, ptIndex) / _getBranchLength(node);
	//printf("radius = endRadius + (base(%.5lf)-endRadius)*interpolation(%.5lf)\n", baseRadius, interpolation);
	return endRadius + interpolation*(baseRadius-endRadius);
}

void CTree::_drawTree3D(CNode3D* node) 
{
	vector<CVec3df*>* line = node->getLine();	
	CVec3df p1, p2, v;
	
	//float mat[4] = {0.8,0.4,0.3,1};
	float mat[4] = {0.239,0.125,0.001,1};
	glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mat);
	
	
	if (node->isTwig()) {
		p1 = *line->at(0);
		p2 = *line->at(1);

		//p1.v[0] = 0;
		//p2.v[0] = 0;

		v = p2-p1;
		CVec3df vn = v.normalise();
		
		p1[Y] -= root3D->getLine()->front()->v[Y] + 0.025;
		p2[Y] -= root3D->getLine()->front()->v[Y] + 0.025;

		p1[X] -= root3D->getLine()->front()->v[X];
		p2[X] -= root3D->getLine()->front()->v[X];

		
		// draw twig
		
		glEnable(GL_TEXTURE_2D);
		glBindTexture(GL_TEXTURE_2D, CTerrain::barkTexture.ID);
		glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
		glPushMatrix();
		glTranslatef(0,-treeMidPoint,0);
		glLineWidth(2);
		glBegin(GL_LINE_STRIP);
		glVertex3fv(p1.getArray());
		glVertex3fv(p2.getArray());
		glEnd();
		glPopMatrix();
		glDisable(GL_TEXTURE_2D);
		
		// draw leaves
		
		// given the length of the branch
		float l = _lineLength3D(&p1, &p2);
		// for a constant interval along the twig, draw a leaf
		for (float j = 0; j < l; j += 0.02) {
			//printf("WE HAVE A LEAF PLACEMENT\n");
			// an assumption is made that each twig has only two nodes
			// therefore the leaf position can be interpolated between and rotated about the first and second/last points
			float ipl = j/l;
			glPushMatrix();
			glTranslatef(p1[X]+v[X]*ipl,
						p1[Y]+v[Y]*ipl - treeMidPoint,
						 p1[Z]+v[Z]*ipl);
			
			//srand ( time(NULL) );
			// rotate twice around (720 degrees)
			glRotatef(720*j/l, vn[X],vn[Y],vn[Z]);
			glRotatef(15*j/l-7.5, 1,0,0);
			
			//_drawLeaf();
			//float mat2[4] = {0.1,0.5,0.0,0.5};
			float mat2[4] = {25.0/255.0, 80.0/255.0, 0.0/255.0, 1};
			float mat3[4] = {0.9,0.1,0.1,1};
			
			//if((int)(j*100) % 4 != 0)
				glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mat2);
			//else
			//	glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mat3);
			//glutSolidSphere(0.01, 5, 5);

			_drawLeaf();
			glPopMatrix();
		}
		
	} else {
		float r1, r2, h;
		for(int i = 0; i < line->size()-1; i++)
		{		
			p1 = *line->at(i);
			p2 = *line->at(i+1);

			//p1.v[0] = 0;
			//p2.v[0] = 0;
			
			// offset y so the trees are on the ground (well, slightly into the ground)
			p1[Y] -= root3D->getLine()->front()->v[Y] + 0.02;
			p2[Y] -= root3D->getLine()->front()->v[Y] + 0.02;

			p1[X] -= root3D->getLine()->front()->v[X];
			p2[X] -= root3D->getLine()->front()->v[X];
			
			// v is the difference vector
			v = CVec3df(p2-p1);
			
			// radius at bottom and top of the current segment
			r1 = _getBranchRadius(node, i);
			r2 = _getBranchRadius(node, i+1);
			
			//printf("i=%d  r1=%.5lf       r2 = %.5lf\n",i, r1,r2);
			h = _lineLength3D(&p1,&p2);
			
			float theta, iota, phi;
			if (h == 0) return;
			theta = -acos(v[Y]/h);
			iota = -atan2(v[Z],v[X]);
			
	#define NUM_CIRCLE_SEGMENTS 8
			CVec3df pA,pB;
			glPushMatrix();
			glTranslatef(p1[X],(p1[Y] - treeMidPoint) ,p1[Z]);
			glRotatef(iota*180.0f/Pi, 0,1,0);
			glRotatef(theta*180.0f/Pi, 0,0,1);
			
			glEnable(GL_TEXTURE_2D);
			glBindTexture(GL_TEXTURE_2D, CTerrain::barkTexture.ID);
			glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
			
			glBegin(GL_QUAD_STRIP);
			for (int j = 0 ; j <= NUM_CIRCLE_SEGMENTS; j++) {
				phi = Pi*2.0*j/NUM_CIRCLE_SEGMENTS;
				float y1 = 0;
				float y2 = h;
				if (i!=0) {
					CVec3df p0 = *line->at(i-1);
					p0[Y] -= root3D->getLine()->front()->v[Y] + 0.02;
					double angle1 = (Pi-lineAngle3D(&p0, &p1)-lineAngle3D(&p0,&p2))/2;
					//y1 -= r1*tan(angle1)*sin(phi);
				}
				if (i!=line->size()-2) {
					CVec3df p3 = *line->at(i+2);
					p3[Y] -= root3D->getLine()->front()->v[Y] + 0.02;
					double angle1 = (Pi-lineAngle3D(&p1, &p2)-lineAngle3D(&p1,&p3))/2;
					//printf("angle = %f\n", angle1);
					//y2 -= r2*tan(angle1)*sin(phi);
				}
				glNormal3f(sin(phi), 0, cos(phi));
				glTexCoord2f(5*r1*phi, y1);
				glVertex3f(r1*sin(phi), y1, r1*cos(phi));
				glTexCoord2f(5*r2*phi, 5*y2);
				glVertex3f(r2*sin(phi), y2, r2*cos(phi));
			}
			glEnd();
			glPopMatrix();
			glDisable(GL_TEXTURE_2D);
		}
	}
	
	
	glFlush();
	
	vector<CNode3D*>* kids = node->getChildren();
	vector<CNode3D*>::iterator kid;
    for ( kid = kids->begin() ; kid != kids->end() ; kid++) _drawTree3D(*kid);
}

// render a rough leaf shape
void CTree::_drawLeaf()
{
	//if(leaf_index==0)
	//{
	//	leaf_index = glGenLists(1);
	//	glNewList(leaf_index, GL_COMPILE);
	//	glColor3f(0,1,0.2);
	//	glutSolidSphere(0.01, 5, 5);
		glBegin(GL_LINES);
			glVertex3f(0,0,0);
			glVertex3f(0.02, 0, 0);
		glEnd();
		glTranslatef(0.02, 0, 0);
		glBegin(GL_TRIANGLE_FAN);
			glVertex3f(0,0,0);
			glVertex3f(0.02,-0.01,-0.005);
			glVertex3f(0.02,0.01,0.005);
			glVertex3f(-0.02,0.01,-0.005);
		glEnd();
	//}
	//glCallList(leaf_index);
}

string CTree::className()
{
	return "Tree";
}

void CTree::regenerate() 
{	
	_discoverTree();
		if (!treeMidPoint)
		treeMidPoint = 0.0;
	if (root3D != NULL)
		delete root3D;
	_make3DTree();
}

void CTree::_make3DTree() 
{
	root3D = _convert2DNodeTo3DNode(root2D);
	_spreadBranchesOut(root3D);
}

//This method spreads the branchs out into the 3D space as far apart from each other as possible
//It is divided into 4 cases corresponding to each of the 4 quadrants
void CTree::_spreadBranchesOut(CNode3D* parent)
{
	//Create an angle vector
	//Find All branches on left, find all on right
	//children on left
	// rand the first child angle between -Pi/4.0 -> Pi/4.0
	// Record its angle
	// For the following children
	//Determine largest gap
	//halve gap and set and record angle
	//Repeat for children on the right
	//recurse for all children
	
	//TEST CASE FOR ROTATION AROUND ARBITRARY AXIS
	/*CVec3df* axis2 = new CVec3df(1,1,1);
	 CVec3df* axis1 = new CVec3df(-1,1,1);
	 CVec3df* vec = new CVec3df(1,0,0);
	 (*root3D->getChildren()->begin())->setAngle(-Pi/4.0);
	 if((*root3D->getChildren()->begin())->getChildren()->size()>0)
	 (*root3D->getChildren()->begin())->getChildren()->at(0)->setAngle(Pi/4.0);
	 
	 vec->rotateAroundAxis(axis1,axis2, Pi);
	 printf("\nvec= X%f Y%f Z%f", vec->v[X], vec->v[Y], vec->v[Z]);*/
	
	srand ( time(NULL) );
	list<double> leftAngles;
	list<double> rightAngles;
	bool leftFirst=true;
	bool rightFirst=true;
	list<double> leftAnglesCopy;
	list<double> rightAnglesCopy;
	bool leftFirstCopy=true;
	bool rightFirstCopy=true;
	vector<CNode3D*> copies;
	vector<CNode3D*>::iterator j;
	//create copies of all sub branches
	for(j = parent->getChildren()->begin(); j < parent->getChildren()->end();j++)
	{
		copies.push_back((*j)->copyNode());
	}
	for(j = copies.begin(); j < copies.end();j++)
	{
		parent->addChild(*j);
	}
	
	for(j = parent->getChildren()->begin(); j < parent->getChildren()->end();j++)
	{
		if((*j)->isLeft() && !(*j)->isCopy())
		{
			if(leftFirst)
			{
				double angle = randomDouble(Pi/4.0, 3.0*Pi/4.0);
				leftAngles.push_back(angle);
				(*j)->setAngle(angle);
				leftFirst = false;
			}
			else 
			{
				
				double max = 3.0*Pi/4.0+0.1;
				double min = Pi/4.0-0.1;
				//int largestUp = -1;
				//int largestDown = -1;
				double largestGap = 0;
				double largestGapAngle = 0;
				list<double>::iterator itL=leftAngles.begin();
				list<double>::iterator itU=leftAngles.begin();
				itU++;
				for (list<double>::iterator it=leftAngles.begin(); it!=leftAngles.end() ; it++)
				{
					double distL = 0;
					double distU = 0;
					
					if (it==leftAngles.begin())
						distL = *it - min;
					else 
					{
						distL = *it - *itL;
						itL++;
					}
					
					if (itU==leftAngles.end()) 
						distU = max - *it;
					else
						distU = *itU - *it;
					
					if (distU > distL && distU > largestGap) {
						largestGap = distU;
						largestGapAngle = largestGap/2.0 + *it;
					}
					else if(distL > largestGap)
					{
						largestGap = distL;
						largestGapAngle = *it - largestGap/2.0;
					}
					if(itU != leftAngles.end())
						itU++;
				}
				//itU++;
				//printf("LargestGapAngle: %f", largestGapAngle);
				leftAngles.push_back(largestGapAngle);
				(*j)->setAngle(largestGapAngle);
				leftAngles.sort(compare_double);
			}
		}
		else if (!(*j)->isLeft() && !(*j)->isCopy())
		{
			if(rightFirst)
			{
				double angle = randomDouble(Pi/4.0, 3.0*Pi/4.0);
				rightAngles.push_back(angle);
				(*j)->setAngle(angle);
				rightFirst = false;
			}
			else 
			{
				
				double max = 3.0*Pi/4.0+0.1;
				double min = Pi/4.0-0.1;
				double largestGap = 0;
				double largestGapAngle = 0;
				list<double>::iterator itL=rightAngles.begin();
				list<double>::iterator itU=rightAngles.begin();
				itU++;
				for (list<double>::iterator it=rightAngles.begin(); it!=rightAngles.end() ; it++)
				{
					double distL = 0;
					double distU = 0;
					
					if (it==rightAngles.begin())
						distL = *it - min;
					else 
					{
						distL = *it - *itL;
						itL++;
					}
					
					if (itU==rightAngles.end()) 
						distU = max - *it;
					else
						distU = *itU - *it;
					
					if (distU > distL && distU > largestGap) {
						largestGap = distU;
						largestGapAngle = largestGap/2.0 + *it;
					}
					else if(distL > largestGap)
					{
						largestGap = distL;
						largestGapAngle = *it - largestGap/2.0;
					}
					if(itU != rightAngles.end())
						itU++;
				}
				//printf("LargestGapAngle: %f", largestGapAngle);
				rightAngles.push_back(largestGapAngle);
				(*j)->setAngle(largestGapAngle);
				rightAngles.sort(compare_double);
			}
			
		}
		else if ((*j)->isLeft() && (*j)->isCopy())
		{
			if(leftFirstCopy)
			{
				double angle = randomDouble(-Pi/4.0, Pi/4.0);
				leftAnglesCopy.push_back(angle);
				(*j)->setAngle(angle);
				leftFirstCopy = false;
			}
			else 
			{
				
				double max = Pi/4.0+0.1;
				double min = -Pi/4.0-0.1;
				//int largestUp = -1;
				//int largestDown = -1;
				double largestGap = 0;
				double largestGapAngle = 0;
				list<double>::iterator itL=leftAnglesCopy.begin();
				list<double>::iterator itU=leftAnglesCopy.begin();
				itU++;
				for (list<double>::iterator it=leftAnglesCopy.begin(); it!=leftAnglesCopy.end() ; it++)
				{
					double distL = 0;
					double distU = 0;
					
					if (it==leftAnglesCopy.begin())
						distL = *it - min;
					else 
					{
						distL = *it - *itL;
						itL++;
					}
					
					if (itU==leftAnglesCopy.end()) 
						distU = max - *it;
					else
						distU = *itU - *it;
					
					if (distU > distL && distU > largestGap) {
						largestGap = distU;
						largestGapAngle = largestGap/2.0 + *it;
					}
					else if(distL > largestGap)
					{
						largestGap = distL;
						largestGapAngle = *it - largestGap/2.0;
					}
					if(itU != leftAnglesCopy.end())
						itU++;
				}
				
				//printf("LargestGapAngle: %f", largestGapAngle);
				leftAnglesCopy.push_back(largestGapAngle);
				(*j)->setAngle(largestGapAngle);
				leftAnglesCopy.sort(compare_double);
			}
		}
		else if (!(*j)->isLeft() && (*j)->isCopy())
		{
			if(rightFirstCopy)
			{
				double angle = randomDouble(-Pi/4.0, Pi/4.0);
				rightAnglesCopy.push_back(angle);
				(*j)->setAngle(angle);
				rightFirstCopy = false;
			}
			else 
			{
				
				double max = Pi/4.0+0.1;
				double min = -Pi/4.0-0.1;
				double largestGap = 0;
				double largestGapAngle = 0;
				list<double>::iterator itL=rightAnglesCopy.begin();
				list<double>::iterator itU=rightAnglesCopy.begin();
				itU++;
				for (list<double>::iterator it=rightAnglesCopy.begin(); it!=rightAnglesCopy.end() ; it++)
				{
					double distL = 0;
					double distU = 0;
					
					if (it==rightAnglesCopy.begin())
						distL = *it - min;
					else 
					{
						distL = *it - *itL;
						itL++;
					}
					
					if (itU==rightAnglesCopy.end()) 
						distU = max - *it;
					else
						distU = *itU - *it;
					
					if (distU > distL && distU > largestGap) {
						largestGap = distU;
						largestGapAngle = largestGap/2.0 + *it;
					}
					else if(distL > largestGap)
					{
						largestGap = distL;
						largestGapAngle = *it - largestGap/2.0;
					}
					if(itU != rightAnglesCopy.end())
						itU++;
				}
				//printf("LargestGapAngle: %f", largestGapAngle);
				rightAnglesCopy.push_back(largestGapAngle);
				(*j)->setAngle(largestGapAngle);
				rightAnglesCopy.sort(compare_double);
			}
			
		}

	}
	for(j = parent->getChildren()->begin(); j < parent->getChildren()->end();j++)
		_spreadBranchesOut(*j);
	
}


//Converts the 2D structure into the 3D representation for the 3D model
CNode3D* CTree::_convert2DNodeTo3DNode(CNode2D* node2D)
{
	vector<CVec2di*>* line2D = node2D->getLine()->getRawControlPoints();
	vector<CVec3df*>* line3D = new vector<CVec3df*>();
	
	if (node2D->getParent() != NULL && !node2D->isTwig()) 
		line3D->push_back(new CVec3df(sketch2WorldProfile(*node2D->getStartPoint())));
		//line3D->push_back(new CVec3df(CSketchManager::sketch2WorldProfile(*node2D->getParent()->getLine()->getRawControlPoints()->at(node2D->getParentIntersection()))));
	vector<CVec2di*>::iterator i;
	for (i = line2D->begin() ; i != line2D->end() ; i++) {
		CVec3df* newPoint3D = new CVec3df(sketch2WorldProfile(**i));
		line3D->push_back(newPoint3D);
	}
	CNode3D* node3D = new CNode3D(line3D);
	if (node2D->getParent() != NULL)
	{
		node3D->setTwig(node2D->isTwig());
		node3D->setLeft(node2D->isLeft());
		node3D->setAxisPoint(new CVec3df(sketch2WorldProfile(*node2D->getAxisPoint())));
		//printf("\nAxisPoint: %i, %i", node2D->getAxisPoint()->v[X], node2D->getAxisPoint()->v[Y]);
	}
		
	vector<CNode2D*>::iterator j;
	for (int j = 0; j < node2D->getChildren()->size(); j++) {
		CNode3D* childNode = _convert2DNodeTo3DNode(node2D->getChildren()->at(j));
		//childNode->setParent(node3D);
		//childNode->setAxisPoint(new CVec3df(CSketchManager::sketch2WorldProfile(*node2D->getAxisPoint())));
		//childNode->setParentIntersection(node2D->getChildren()->at(j)->getParentIntersection());
		node3D->addChild(childNode);
	}
	

	return node3D;
}

void CTree::_discoverTree()
{
	SketchesSeen.clear();
	SketchFoliage.clear();
	//find the lowest y pos root
	CVec2di* lowestPoint;
	
	int lowestSketchIndex;
	bool forward;
	for(int i = 0; i < sketches->size(); i++)
	{
		//set up the sketches seen so that everything is unseen
		SketchesSeen.push_back(false);
		//start point of line
		CVec2di* cp1 = (*sketches)[i]->getRawControlPoints()->front();
		//end point of line
		CVec2di* cp2 = (*sketches)[i]->getRawControlPoints()->back();
		
		//if start is lower than lowest point
		if(i == 0 || cp1->v[1] < lowestPoint->v[1])
		{
			lowestPoint = cp1;
			lowestSketchIndex = i;
			forward = true;
		}
		//drawing could be upsidedown so check last point lower than lowest point
		if( cp2->v[1] < lowestPoint->v[1])
		{
			lowestPoint = cp2;
			lowestSketchIndex = i;
			forward = false;
		}
		
		bool foliage = false;
		for(int j = 0; j < sketches->size(); j++)
			if((*sketches)[j]->isClosed())
				foliage = true;
		SketchFoliage.push_back(foliage);
	}
	
	if (root2D != NULL) {
		delete root2D;
	}
	
	root2D = new CNode2D(sketches->at(lowestSketchIndex), forward);
	_recurseTree(lowestSketchIndex, root2D);
	
	_generateFoliage();
	//printf("\nRecursion Complete\n");
	//NOTIFY DONE 2D-NESS
}

//This method starts the foliage intersection search for each closed sketch
void CTree::_generateFoliage()
{
	//Foliage Calculations
	
	for(int i = 0; i < sketches->size(); i++)
	{
		CSketch* s = sketches->at(i);
		if (s->isClosed()) {
			//Then we have a foliage sketch...
			
			//Compute the center of mass
			//to massively speed up computation later, we also create an axis aligned bounding box
			vector<CVec2di> boundingBox;
			CVec2di cm(0,0);
			int xMin = 10000;
			int yMin = 10000;
			int xMax = 0;
			int yMax = 0;
			for(int j = 0; j< s->getRawControlPoints()->size(); j++)
			{
				CVec2di* cp = s->getRawControlPoints()->at(j);
				cm.v[X] += cp->v[X];
				cm.v[Y] += cp->v[Y];
				
				
				
			}
			cm.v[X] /= s->getRawControlPoints()->size();
			cm.v[Y] /= s->getRawControlPoints()->size();
			
			//We want to make a twig go to each control point to an appropriate branch
			//We also need to know information about the area of the point cloud.
			double av_distance = 0;
			for(int j = 0; j< s->getRawControlPoints()->size(); j++)
			{
				CVec2di* cp = s->getRawControlPoints()->at(j);
				av_distance += _lineLength(&cm, cp);
			}
			av_distance /= s->getRawControlPoints()->size();
			// av_distance is a measure of the average distance of each cp from the center of mass
			
			// need some measure of control point density
			// simply number of control points over the average_distance as a rough guide
			double density =  (double)s->getRawControlPoints()->size() / (double)av_distance;
			
			_recurseLookForFoliageIntersection(root2D, s, false, density, av_distance, &cm);
			
			//printf("Density of foliage: %f, AvDist: %f", density, av_distance);
			
			// if av_distance is large then chances are likely that the foliage cover is over large area 
			
			// any branch that enters region is a potential, including its children
			// any branch that travels through a region less than 30 from center of mass then consider it a potential
			// if few number of branches come through area then foliage could be localised?
			//this is the slow part!!!
			//vector<CNode2D*> potentialNodes;
			
			
		}
	}
}


//This Method determines where foliage starts and stops on the Branches. It also creates the twigs if foliage is found
//The only way this will work is if the trunk begins outside the foliage region.
void CTree::_recurseLookForFoliageIntersection(CNode2D* node, CSketch* foliage, bool insideRegion, double density, double av_distance, CVec2di* centerOfMass)
{
	bool inReg = insideRegion;
	bool foundFoliage = inReg;
	int startFoliage = -1;
	int endFoliage = -1;
	//printf("\nLineLength: %f, av_dist: %f", _lineLength(node->getLine()->getRawControlPoints()->front(), centerOfMass) , av_distance);
	if (_lineLength(node->getLine()->getRawControlPoints()->front(), centerOfMass) < av_distance ) {
		inReg = true;
		foundFoliage = true;
	}
	
	for(int i = 0; i < foliage->getRawControlPoints()->size(); i++)
	{
		CVec2di* cpi1;
		CVec2di* cpi2 = foliage->getRawControlPoints()->at(i);
		if (i==0)
			cpi1 = foliage->getRawControlPoints()->back();
		else
			cpi1 = foliage->getRawControlPoints()->at(i-1);
		
		//printf("\nCPi1: %d, %d", cpi1->v[X], cpi1->v[Y]);
		//printf("\nCPi2: %d, %d", cpi2->v[X], cpi2->v[Y]);
		//calculate intersection points
		for(int j = 1; j< node->getLine()->getRawControlPoints()->size(); j++)
		{
			CVec2di* cpj1 = node->getLine()->getRawControlPoints()->at(j-1);
			CVec2di* cpj2 = node->getLine()->getRawControlPoints()->at(j);
			//printf("\nCPj1: %d, %d", cpj1->v[X], cpj1->v[Y]);
			//printf("\nCPj2: %d, %d", cpj2->v[X], cpj2->v[Y]);
			if(_internalIntersectionPoint(cpj1, cpj2, cpi1, cpi2))
			{
				//We have an intersection so that means that we should record that this branch is inside the foliage region
				inReg = !inReg;
				foundFoliage = true;
				
				if(startFoliage < 0)
				{
					if(inReg)
						startFoliage = j-1;
					else
						endFoliage = j;
				}
				
				//we could start foliage creation until insideRegion == false?
				//BEGIN FOLIAGE CREATION
				
				
				//otherwise we record where the foliage starts and ends and then comeback later to populate it...
				
			}
		}
	}
	
	if(foundFoliage)
	{
		if(endFoliage == -1)
			endFoliage = node->getLine()->getRawControlPoints()->size()-1;
		
		if(startFoliage == -1)
			startFoliage = 0;
		double d = 1.0 / density;
		if (d > 3) {
			d = 3;
		}
		d /= 3;
		d*= 25;
		d+=5; //d will be between 5 and 30;
		
		double a = av_distance;
		if (a > 50) 
			a = 50;
		if( a<30)
			a = 30;
		a -= 30;
		
		for(int i = startFoliage; i < endFoliage; i++)
		{
			CVec2di* cpStart = node->getLine()->getRawControlPoints()->at(i);
			CVec2di* cpEnd = node->getLine()->getRawControlPoints()->at(i+1);
			
			double lineLength = _lineLength(cpStart, cpEnd);
			double it = lineLength / d;
			for(int j = 0; j < it; j++)
			{
				CVec2di* newPoint = _pointOnLine(cpStart, cpEnd, (1.0/it)*j);
				CSketch* sk = new CSketch();
				int l = randomInt(0, 1);
				if(l==0)
				{
					CVec2di* endPoint = _lineEndAtAngle(newPoint, cpEnd , -Pi/6.0, av_distance);
					sk->getRawControlPoints()->push_back(newPoint);
					sk->getRawControlPoints()->push_back(endPoint);
					node->addChild(new CNode2D(node, sk, newPoint, cpEnd, true, false));
				}
				else 
				{
					CVec2di* endPoint = _lineEndAtAngle(newPoint, cpEnd , Pi/6.0, av_distance);
					sk->getRawControlPoints()->push_back(newPoint);
					sk->getRawControlPoints()->push_back(endPoint);
					node->addChild(new CNode2D(node, sk, newPoint, cpEnd, true, true));
				}
			}
		}
	}
	
	
	for(int i = 0; i < node->getChildren()->size(); i++)
	{
		if (!node->getChildren()->at(i)->isTwig()) {
			_recurseLookForFoliageIntersection(node->getChildren()->at(i), foliage, /*inReg*/ false, density, av_distance, centerOfMass);
		}
		
	}
}



//Creates the Tree structure by analysing the open sketches.
#define CP4S(x) (*sketches->at(x)->getRawControlPoints())
void CTree::_recurseTree(int sketchIndex, CNode2D* parentNode)
{
	CVec2di* cp[2];
	SketchesSeen[sketchIndex] = true;
	cp[0] = (*sketches)[sketchIndex]->getRawControlPoints()->front();
	//end point of line
	cp[1] = (*sketches)[sketchIndex]->getRawControlPoints()->back();
	bool startFromEnd = false;
	if(cp[0]->v[1] > cp[1]->v[1])
	{
		startFromEnd = true;
	}
	vector<int> sketchIndices;
	
	int inserted = 0;
	if (!startFromEnd) {
		for (int i = 0; i < CP4S(sketchIndex).size()-1; i++) {
			for (int j = 0; j < sketches->size(); j++) {
				if (!SketchesSeen[j] && !sketches->at(j)->isClosed() ) {
					double dist = _distToLine( CP4S(j)[0], CP4S(sketchIndex)[i],  CP4S(sketchIndex)[i+1]);
					bool internal = _internalPoint(CP4S(j)[0], CP4S(sketchIndex)[i], CP4S(sketchIndex)[i+1]);
					if(internal && dist < 20)
					{
						//printf("\n%f\t%f", dist, _lineLength(CP4S(j)[0],  CP4S(sketchIndex)[i]));
						dist += 1;
						SketchesSeen[j] = true;
						
						if (_lineLength(CP4S(j)[0],  CP4S(sketchIndex)[i]) < 3) {
							//connect to start
							CNode2D* node = new CNode2D(parentNode, i, sketches->at(j), true, false);
							SketchesSeen[j] = true;
							sketchIndices.push_back(j);
							//_recurseTree(j, node);
						}
						else if (_lineLength(CP4S(j)[0], CP4S(sketchIndex)[i+1]) < 5) {
							//connect to end
							CNode2D* node = new CNode2D(parentNode, i+1, sketches->at(j), true, false);
							//_recurseTree(j, node);
							SketchesSeen[j] = true;
							sketchIndices.push_back(j);
						}
						else {
							//create new control point at intersection
							CVec2di* intersection = _intersectionPoint(CP4S(j)[0], CP4S(j)[1], CP4S(sketchIndex)[i], CP4S(sketchIndex)[i+1]);
							vector<CVec2di*>* vec =  parentNode->getLine()->getRawControlPoints();//sketches->at(sketchIndex)->getRawControlPoints();
							
							vector<CVec2di*>::iterator it = vec->begin();
							//TODO: This is still a hardcore bug (i.e points inserted in wrong place)
							vec->insert(it+i+1+inserted, intersection);
							//sketches->at(j)->getRawControlPoints()->insert( sketches->at(j)->getRawControlPoints()->begin(), intersection);
							//printf("\nIntersection Point [%d]: %d, %d", i+1+inserted, intersection->v[0], intersection->v[1]);
							CNode2D* node = new CNode2D(parentNode, i+1+inserted, sketches->at(j), true, false);
							inserted++;
							//_recurseTree(j, node);
							SketchesSeen[j] = true;
							sketchIndices.push_back(j);
						}
					}
				}
			}
		}
	}
	for(int i = 0; i < parentNode->getChildren()->size(); i++)
	{
		_recurseTree(sketchIndices[i], parentNode->getChildren()->at(i));
	}
	
	
}

double CTree::_distToLine(CVec2di* point, CVec2di* p1, CVec2di* p2)
{	
	double theta = lineAngle(p1, p2);
	double phi = lineAngle(p1, point);
	double angle = phi-theta;
	//printf("theta: %f, phi: %f, angle %f\n",theta,phi,angle);
	
	double ret = _lineLength(point, p1)*sin(angle);
	if(ret < 0)
		ret *= -1;
	return ret;
}

bool CTree::_internalPoint(CVec2di* point, CVec2di* p1, CVec2di* p2)
{
	//bool internal = true;
	double theta = lineAngle(p1,p2);
	double phi = lineAngle(p1, point);
	double angle1 = fabs(phi-theta);
	if (angle1 > (Pi/2.0f) && angle1 < (3.0f*Pi/2.0f))
		return false;
	
	theta = lineAngle(p2, p1);
	phi = lineAngle(p2, point);
	double angle3 = fabs(phi-theta);
	if (angle3 > (Pi/2.0f) && angle3 < (3.0f*Pi/2.0f))
		return false;
	//printf("\nA1:%f A3:%f val:%d",angle1,angle3,internal);
	
	return true;
}

bool CTree::_internalIntersectionPoint(CVec2di* q1, CVec2di* q2, CVec2di* p1, CVec2di* p2)
{
	CVec2di* point = _intersectionPoint(q1, q2, p1, p2);
	CVec2di* qs = q1->v[X] < q2->v[X] ? q1 : q2;
	CVec2di* qb = q1->v[X] < q2->v[X] ? q2 : q1;
	CVec2di* ps = p1->v[X] < p2->v[X] ? p1 : p2;
	CVec2di* pb = p1->v[X] < p2->v[X] ? p2 : p1;
	
	if((point->v[X] >= qs->v[X] && point->v[X] <= qb->v[X]) && (point->v[X] >= ps->v[X] && point->v[X] <= pb->v[X]))
	{
		CVec2di* qs1 = q1->v[Y] < q2->v[Y] ? q1 : q2;
		CVec2di* qb1 = q1->v[Y] < q2->v[Y] ? q2 : q1;
		CVec2di* ps1 = p1->v[Y] < p2->v[Y] ? p1 : p2;
		CVec2di* pb1 = p1->v[Y] < p2->v[Y] ? p2 : p1;
		if((point->v[Y] >= qs1->v[Y] && point->v[Y] <= qb1->v[Y]) && (point->v[Y] >= ps1->v[Y] && point->v[Y] <= pb1->v[Y]))
		{
			delete point;
			return true;
		}
	}
	delete point;
	return false;
}

CVec2di* CTree::_intersectionPoint(CVec2di* q1, CVec2di* q2, CVec2di* p1, CVec2di* p2)
{
	int x1 = (q1->v[0] - q2->v[0]);
	int y1 = (q1->v[1] - q2->v[1]);
	
	if (x1==0) {
		x1+=0.000001;
	}
	double m1 = (double)y1/(double)x1;
	double c1 = (double)q1->v[1] - (m1*(double)q1->v[0]);
	
	double x2 = (double)(p1->v[0] - p2->v[0]);
	double y2 = (double)(p1->v[1] - p2->v[1]);
	if(x2==0)
	{
		x2+=0.000001;
	}
	double m2 = y2/x2;
	
	double c2 = (double)p1->v[1] - (m2*(double)p1->v[0]);
	
	double x = (c1-c2)/(m2-m1);
	double y = m1*x + c1;
	CVec2di* point = new CVec2di((int)x,(int)y);
	
	/*if(point->v[X] < 0 || point->v[Y] < 0)
		printf("INTERSECTION FAIL");*/
	return point;
}

double CTree::_lineLength(CVec2di* p1, CVec2di* p2)
{
	///((x-x)^2 + (y-y)^2)^.5
	double x = p1->v[0] - p2->v[0];
	double y = p1->v[1] - p2->v[1];
	return sqrt((x*x) + (y*y));	
}

CVec2di* CTree::_pointOnLine(CVec2di* p1, CVec2di* p2, double percentage)
{	
	CVec2di vec = *p2-*p1;
	return new CVec2di((percentage*vec)+*p1);
}
											
CVec2di* CTree::_lineEndAtAngle(CVec2di* p1, CVec2di* p2, double angle, double length)
{
	//if left then create line so that angle is less than 
	
	//CMatrix2df(float a11, float a12, float a21, float a22);
	CVec2di vec = *p2-*p1;
	//vec.normaliseDestructive();
	CVec2df vecf((float)vec.v[X], (float)vec.v[Y]);
	CVec2df p1f((float)p1->v[X], (float)p1->v[Y]);
	CMatrix2df rot(cos(angle), -1.0*sin(angle), sin(angle), cos(angle));
	
	CVec2df newVec = rot*vecf;
	
	newVec.normaliseDestructive();
	CVec2df point = (length * newVec) + p1f;
	
	return new CVec2di((int)point.v[X], (int)point.v[Y]);
}

double CTree::_lineLength3D(CVec3df* p1, CVec3df* p2)
{
	///((x-x)^2 + (y-y)^2)^.5
	double x = p1->v[0] - p2->v[0];
	double y = p1->v[1] - p2->v[1];
	double z = p1->v[2] - p2->v[2];
	return sqrt((x*x) + (y*y) + (z*z));	
}

CVec3df CTree::sketch2WorldProfile(CVec2di s)
{
	float x = (float)(s[X]-sketch_area_width/2)/sketch_area_width;
	float y = (float)s[Y]/sketch_area_height;
	float z = 0;
	
	return CVec3df(x,y,z);
}
}